#if HAVE_CONFIG_H
#include "config.h"
#endif

#include <aio_misc.h>
#include <errno.h>
#include <assert.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/time.h>
#include <time.h>
#include <unistd.h>
#include <aio.h>

#define AIO_PER_ALLOCATION 32

/* Chain of requests being processed */

rtems_chain_control aio_requestlist;

/* The request pool */

rtems_chain_control aio_req_pool;

/* Requests waiting to be processed */

rtems_chain_control aio_runlist;

/* Number of threads currently running */
static int aio_nthreads;

/* Number of threads waiting for work to arrive */
static int aio_idle_thread_count;

static size_t aio_pool_max_size;
static size_t aio_pool_size;

/* Sgi's aioinit structure,
 * need to add RTEMS default
 * optimizations.
 *
 * Defaults are taken from glibc implementation
 * need to set rtems defaults.
 * See aio.h 
 */

static struct aioinit optim = {
  15,                           /* Maximal number of threads */
  64,                           /* Number of expected simultaneous requests */
  0,
  0,
  0,
  0,
  1,
  0
};


/*
 * Need this structure for the runlist queue & remove.
 */

typedef struct rtems_aio_wrapper
{

  rtems_chain_node *chain;      /* Chain of requests having the same FD */
  rtems_chain_node *node;       /* The request in the perfd chain */
  rtems_chain_node runp;        /* The link in the runlist chain */

} rtems_aio_wrapper;

pthread_mutex_t aio_requests_mutex;
pthread_cond_t aio_new_request_notification = PTHREAD_COND_INITIALIZER;

static void *rtems_aio_handle (void *arg);
static void rtems_aio_to_runlist (rtems_aio_wrapper * chn);

/* 
 *  rtems_aio_init
 *
 * SGI's aio_init for optimization 
 * Need to add RTEMS specific optimizations
 *
 *  Input parameters:
 *        init - struct of optimization values
 *
 *  Output parameters: NONE
 *        
 */

int
rtems_aio_init (const struct aioinit *init)
{

  pthread_mutexattr_t attr;

  pthread_mutexattr_init (&attr);
  pthread_mutexattr_settype (&attr, PTHREAD_MUTEX_RECURSIVE);
  pthread_mutex_init (&aio_requests_mutex, &attr);

  rtems_chain_initialize_empty (&aio_req_pool);
  rtems_chain_initialize_empty (&aio_requestlist);
  rtems_chain_initialize_empty (&aio_runlist);

  if (init != NULL) {

    pthread_mutex_lock (&aio_requests_mutex);

    if (rtems_chain_is_empty (&aio_req_pool)) {
      optim.aio_threads = init->aio_threads < 1 ? 1 : init->aio_threads;
      optim.aio_num = (init->aio_num < AIO_PER_ALLOCATION
                       ? AIO_PER_ALLOCATION : init->aio_num & ~AIO_PER_ALLOCATION);

    }

    if (init->aio_idle_time != 0)
      optim.aio_idle_time = init->aio_idle_time;

    pthread_mutex_unlock (&aio_requests_mutex);

  }
  return 0;

}

/* 
 *  rtems_aio_get_elem
 *  
 * Creates the request pool and manages slots
 *
 *  Input parameters: NONE
 *
 *  Output parametes: 
 *        request - pointer to available slot
 */

rtems_aio_request *
rtems_aio_get_elem (void)
{

  rtems_aio_request *reqfd = NULL;
  rtems_chain_node *node;
  int i;

  /* if the current pool is filled up add more slots 
   * need some hard limit to force MAX pool size   
   */

  if (aio_pool_size + 1 >= aio_pool_max_size) {
    for (i = 0; i < AIO_PER_ALLOCATION; i++) {

      reqfd = malloc (sizeof (rtems_aio_request));

      if (!reqfd)
        return NULL;

      else
        rtems_chain_append (&aio_req_pool, &reqfd->next_buf);

    }
    aio_pool_max_size = aio_pool_max_size + AIO_PER_ALLOCATION;
  }

  /* Take one slot from the aio_req_pool and use it to process a request */
  aio_pool_size++;

  node = aio_req_pool.last;
  rtems_chain_extract (node);
  reqfd = (rtems_aio_request *) node;
  rtems_chain_prepend (&aio_req_pool, &reqfd->next_buf);

  return reqfd;

}

/* 
 *  rtems_aio_find_req
 *
 * Searches request based on struct aiocb
 *
 *  Input parameters:
 *        aiocbp - pointer to asynchronous I/O control block
 *
 *  Output parameters:
 *        pointer to node - if request found
 *        NULL            - otherwise
 */

rtems_chain_node *
rtems_aio_find_req (struct aiocb * aiocbp)
{

  rtems_chain_node *node = aio_requestlist.first;
  rtems_aio_chain *perfd;
  rtems_aio_request *perprio;
  int fildes = aiocbp->aio_fildes;
  rtems_chain_control *chain;

  /* Requests are organized in a chain of chains based on their FD */

  perfd = (rtems_aio_chain *) node;
  while (!rtems_chain_is_tail (&aio_requestlist, node) && perfd->fd < fildes) {
    node = node->next;
    perfd = (rtems_aio_chain *) node;
  }

  /* Check chain of requests having FD = aiocbp->fildes */

  if (rtems_chain_is_tail (&aio_requestlist, node))
    return NULL;
  else {

    if (perfd->fd != fildes)
      return NULL;

    else {
      chain = &perfd->perfd;
      node = chain->first;
      perprio = (rtems_aio_request *) node;

      while (!rtems_chain_is_tail (chain, node) && perprio->aiocbp != aiocbp) {
        node = node->next;
        perprio = (rtems_aio_request *) node;
      }

      if (perprio->aiocbp == aiocbp)
        return node;
    }
  }

  return NULL;

}

/* 
 *  rtems_aio_find_fd
 * 
 * Searches for requests for a given FD
 *
 *  Input parameters: 
 *        filedes - the file descriptor
 *
 *  Output parameters:
 *        pointer to node - if requests found
 *        NULL            - otherwise
 */

rtems_chain_node *
rtems_aio_find_fd (int fildes)
{

  rtems_chain_node *node;
  rtems_aio_chain *perfd;

  node = aio_requestlist.first;
  perfd = (rtems_aio_chain *) node;

  while (!rtems_chain_is_tail (&aio_requestlist, node) && perfd->fd < fildes) {
    node = node->next;
    perfd = (rtems_aio_chain *) node;
  }

  return (!rtems_chain_is_tail (&aio_requestlist, node)
          && perfd->fd == fildes ? node : NULL);

}

/*
 *  rtems_aio_remove_request
 * 
 * Remove a request or a chain of requests
 *
 *  Input parameters:
 *        last - node containing a chain for a given FD
 *        req  - the actual request to remove
 *        all  - if 1 we remove the whole chain
 *
 *  Output parameters: NONE
 */

void
rtems_aio_remove_request (rtems_chain_node * last, rtems_chain_node * req,
                          int all)
{

  rtems_chain_node *temp;
  rtems_aio_chain *aux;

  assert (((rtems_aio_request *) req)->running == AIO_YES
          || ((rtems_aio_request *) req)->running == AIO_QUEUED
          || ((rtems_aio_request *) req)->running == AIO_DONE);

  aux = (rtems_aio_chain *) last;

  if (!rtems_chain_is_head (&aux->perfd, req->previous)) {
    if (!all) {

      /*
       *  If it's not the first request in the chain 
       *  and we don't have to delete all requests of the chain
       */
      rtems_chain_extract (req);
    } else {

      /* Remove all requests in the chain following req */

      while (!rtems_chain_is_tail (&aux->perfd, req)) {
        rtems_chain_node *node_next = req->next;
        rtems_chain_extract (req);
        req = node_next;
      }
    }
  } else {

    /* Remove the chain of requests */
    if (all || rtems_chain_is_tail (&aux->perfd, req->next)) {
      rtems_chain_extract (last);

    } else {
      /* Remove request */
      ((rtems_aio_request *) req->next)->running = AIO_YES;
      rtems_chain_extract (req);
    }

    /* Also remove request from the runlist */

    if (((rtems_aio_request *) req)->running == AIO_YES) {
      temp = aio_runlist.first;
      rtems_aio_wrapper *chn = (rtems_aio_wrapper *) temp;

      while (!rtems_chain_is_tail (&aio_runlist, temp)) {
        if (rtems_chain_are_nodes_equal (chn->node, req))
          break;
        else
          temp = temp->next;
      }

      if (rtems_chain_are_nodes_equal (chn->node, req))
        rtems_chain_extract (temp);

    }
  }
}

/*
 *  rtems_aio_free_request
 *
 * The free requests are added to the end of the aio_req_pool chain
 * while the requests from the request list are at the front 
 * of the aio_req_pool. Whenever a request finishes the work
 * it is added to the end of the aio_req_pool chain.
 *
 *  Input parameters:
 *        elem - the request to be freed
 *
 *  Output parameters: NONE 
 */

void
rtems_aio_free_request (rtems_aio_request * elem)
{

  aio_pool_size--;
  elem->running = AIO_NO;
  rtems_chain_extract (&elem->next_buf);
  rtems_chain_append (&aio_req_pool, &elem->next_buf);

}

/* 
 *  rtems_aio_enqueue_requests
 *
 * Enqueue requests, and creates threads to process them 
 *
 *  Input parameters:
 *        aiocbp     - asynchronous I/O control block
 *        operation  - see aio.h
 * 
 *  Output parameters: 
 *        request    - pointer to a request for the given aiocb
 *        NULL       - if the pool is full
 */

rtems_aio_request *
rtems_aio_enqueue_request (struct aiocb *aiocbp, int operation)
{

  rtems_chain_node *node;
  rtems_chain_node *last_node;
  rtems_chain_control *chain;
  int running = AIO_NO;
  int result = 0;
  rtems_aio_request *reqfd;
  rtems_aio_chain *newfd;
  rtems_aio_chain *first;
  int prio, policy;
  struct sched_param param;
  rtems_aio_wrapper *chn;

  if (aiocbp->aio_reqprio < 0 || aiocbp->aio_reqprio > AIO_PRIO_DELTA_MAX) {
    /* Invalid priority value.  */
    //    rtems_set_errno_and_return_minus_one (EINVAL);  need to check out with mentor
    aiocbp->error_code = EINVAL;
    aiocbp->return_value = -1;
    return NULL;
  }

  /* Compute priority for this request.  */
  pthread_getschedparam (pthread_self (), &policy, &param);
  prio = param.sched_priority - aiocbp->aio_reqprio;

  prio = aiocbp->aio_reqprio;
  pthread_mutex_lock (&aio_requests_mutex);

  last_node = NULL;
  node = aio_requestlist.first;
  first = (rtems_aio_chain *) node;

  /* Search if there is a chain for the current FD */

  while (!rtems_chain_is_tail (&aio_requestlist, node)
         && first->fd < aiocbp->aio_fildes) {
    last_node = node;
    node = node->next;
    first = (rtems_aio_chain *) node;
  }

  chn = malloc (sizeof (rtems_aio_wrapper));

  if (!chn)
    return NULL;

  /* Request a slot on the aio_req_pool */

  reqfd = rtems_aio_get_elem ();

  /* If pool is full */
  if (reqfd == NULL) {
    pthread_mutex_unlock (&aio_requests_mutex);
    return NULL;
  }

  /* Fill in the request */
  reqfd->aiocbp = aiocbp;
  aiocbp->prio = prio;
  aiocbp->policy = policy;
  aiocbp->aio_lio_opcode = operation;
  aiocbp->error_code = EINPROGRESS;
  aiocbp->return_value = 0;
  reqfd->caller_thread = pthread_self();
  rtems_chain_initialize_empty (&reqfd->waitlist);

  /* If there is a chain for the current FD 
   * add request according to priority
   */
  if (first->fd == aiocbp->aio_fildes) {
    rtems_aio_request *temp;
    last_node = NULL;
    newfd = (rtems_aio_chain *) node;
    chain = &newfd->perfd;

    node = chain->first;
    temp = (rtems_aio_request *) node;

    while (!rtems_chain_is_tail (chain, node) && temp->aiocbp->prio >= prio) {
      last_node = node;
      node = node->next;
      temp = (rtems_aio_request *) node;
    }

    if (last_node == NULL)
      rtems_chain_prepend (chain, &reqfd->next_prio);
    else
      rtems_chain_insert (last_node, &reqfd->next_prio);

    running = AIO_QUEUED;
  }

  /* Else create chain and add request on the chain */
  else {
    running = AIO_YES;

    newfd = malloc (sizeof (rtems_aio_chain));
    newfd->fd = aiocbp->aio_fildes;
    rtems_chain_initialize_empty (&newfd->perfd);

    if (last_node == NULL)
      rtems_chain_prepend (&aio_requestlist, &newfd->next_fd);

    else
      rtems_chain_insert (last_node, &newfd->next_fd);

    rtems_chain_prepend (&newfd->perfd, &reqfd->next_prio);

  }

  chn->chain = &newfd->next_fd;
  chn->node = &reqfd->next_prio;

  /* If chain created than start a thread for processing requests */
  if (running == AIO_YES) {
    pthread_t thid;
    if (aio_nthreads < optim.aio_threads && aio_idle_thread_count == 0) {
      running = reqfd->running = AIO_ALLOCATED;

      pthread_attr_t attr;
      pthread_attr_init (&attr);

      pthread_attr_setdetachstate (&attr, PTHREAD_CREATE_DETACHED);

      result = pthread_create (&thid, &attr, rtems_aio_handle, (void *) chn);
      pthread_attr_destroy (&attr);

      if (result == 0)
        ++aio_nthreads;
      else {
        running = reqfd->running = AIO_YES;

        if (aio_nthreads == 0)
          rtems_aio_remove_request (&newfd->next_fd, &reqfd->next_prio, 0);
        else
          result = 0;

      }
    }
  }

  /* Thread not created, add request to runlist */
  if (running == AIO_YES && result == 0) {
    rtems_aio_to_runlist (chn);

    /* check if there are idle threads waiting for work */
    if (aio_idle_thread_count > 0)
      pthread_cond_signal (&aio_new_request_notification);

  }

  if (result == 0)
    reqfd->running = running;

  else {

    rtems_aio_free_request (reqfd);
    aiocbp->error_code = result;
    reqfd = NULL;
  }

  pthread_mutex_unlock (&aio_requests_mutex);

  return reqfd;

}

/*
 *  rtems_aio_handle
 *
 * Work being done by the threads 
 *
 *  Input parameters: 
 *        arg  -  containing node and chain control for the
 *                request being processed
 *
 *             -  NULL if the thread was created to take
 *                work from the runlist
 *
 *  Output parameters: NONE
 */

static void *
rtems_aio_handle (void *arg)
{

  pthread_t self = pthread_self ();
  rtems_aio_wrapper *chn = (rtems_aio_wrapper *) arg;
  rtems_chain_node *node;
  rtems_chain_node *chain;
  int policy;
  struct sched_param param;

  struct aiocb *aiocbp;

  int fildes;

  pthread_getschedparam (self, &policy, &param);

  do {

    /* If function is called to work on the runlist */
    if (arg == NULL)
      pthread_mutex_lock (&aio_requests_mutex);

    /* If function created to run specific work */
    else {
      node = chn->node;
      chain = chn->chain;
      rtems_aio_request *runp = (rtems_aio_request *) node;

      assert (runp->running == AIO_ALLOCATED);

      aiocbp = runp->aiocbp;
      fildes = aiocbp->aio_fildes;

      if (aiocbp->prio != param.sched_priority || aiocbp->policy != policy) {
        param.sched_priority = aiocbp->prio;
        policy = aiocbp->policy;
        pthread_setschedparam (self, policy, &param);
      }

      if (aiocbp->aio_lio_opcode == LIO_READ) {
        aiocbp->return_value = pread (fildes, (void *)
                                      aiocbp->aio_buf,
                                      aiocbp->aio_nbytes, aiocbp->aio_offset);

      } else if (aiocbp->aio_lio_opcode == LIO_WRITE) {

        aiocbp->return_value = pwrite (fildes, (void *)
                                       aiocbp->aio_buf,
                                       aiocbp->aio_nbytes,
                                       aiocbp->aio_offset);

      } else {
        aiocbp->return_value = -1;
        errno = EINVAL;
      }

      pthread_mutex_lock (&aio_requests_mutex);

      if (aiocbp->return_value == -1)
        aiocbp->error_code = errno;
      else
        aiocbp->error_code = 0;

      rtems_aio_notify ((rtems_aio_request *) node);

      assert (runp->running == AIO_ALLOCATED);
      runp->running = AIO_DONE;

      rtems_aio_remove_request (chain, node, 0); 

      /* If we still have requests on the chain */
      if (!rtems_chain_is_tail
          (&((rtems_aio_chain *) chain)->perfd, node->next)) {
        chn->node = node->next;
        rtems_aio_to_runlist (chn);
      }

      rtems_aio_free_request ((rtems_aio_request *) node);

    }

    /* take work from the runlist */
    if (!rtems_chain_is_empty (&aio_runlist))
      node = aio_runlist.first;
    else
      node = NULL;

    /* if runlist empty wait for work */
    if (rtems_chain_is_empty (&aio_runlist) && (optim.aio_idle_time >= 0)) {
      struct timeval now;
      struct timespec wakeup_time;

      ++aio_idle_thread_count;
      gettimeofday (&now, NULL);
      wakeup_time.tv_sec = now.tv_sec + optim.aio_idle_time;
      wakeup_time.tv_nsec = now.tv_usec * 1000;

      if (wakeup_time.tv_nsec > 1000000000) {
        wakeup_time.tv_nsec -= 1000000000;
        ++wakeup_time.tv_sec;
      }

      pthread_cond_timedwait (&aio_new_request_notification,
                              &aio_requests_mutex, &wakeup_time);
      --aio_idle_thread_count;

      if (!rtems_chain_is_empty (&aio_runlist))
        node = aio_runlist.first;
      else
        node = NULL;
    }

    if (node == NULL)
      --aio_nthreads;

    /* Handle next request from the runlist */
    else {
      chn = (rtems_aio_wrapper *) node;

      assert (((rtems_aio_request *) chn->node)->running == AIO_YES);

      ((rtems_aio_request *) chn->node)->running = AIO_ALLOCATED;

      rtems_chain_extract (node);

      if (!rtems_chain_is_empty (&aio_runlist)) {

        if (aio_idle_thread_count > 0)
          pthread_cond_signal (&aio_new_request_notification);

        else if (aio_nthreads < optim.aio_threads) {
          pthread_t thid;

          pthread_attr_t attr;

          pthread_attr_init (&attr);
          pthread_attr_setdetachstate (&attr, PTHREAD_CREATE_DETACHED);

          if (pthread_create (&thid, &attr, rtems_aio_handle, NULL) == 0)
            ++aio_nthreads;

        }

      }
    }

    pthread_mutex_unlock (&aio_requests_mutex);
  }
  while (node != NULL);
  /* Until runlist empty */

  free (chn);
  return NULL;

}

/* 
 *  rtems_aio_to_runlist
 *
 * Add request to runlist 
 *
 *  Input parameters:
 *        chn  - structure containing request see rtems_aio_wrapper
 *
 *  Output parameters: NONE
 */

static void
rtems_aio_to_runlist (rtems_aio_wrapper * chn)
{

  int prio;
  rtems_aio_wrapper *aux;
  rtems_aio_request *req;

  prio = ((rtems_aio_request *) chn->node)->aiocbp->prio;

  rtems_chain_node *first = aio_runlist.first;
  aux = (rtems_aio_wrapper *) first;
  req = (rtems_aio_request *) aux->node;

  if (rtems_chain_is_empty (&aio_runlist) || req->aiocbp->prio < prio)

    rtems_chain_prepend (&aio_runlist, &chn->runp);

  else {
    /* add request to the chain by priority */
    while (!rtems_chain_is_tail (&aio_runlist, first)
           && req->aiocbp->prio >= prio) {
      first = first->next;
      req = (rtems_aio_request *) ((rtems_aio_wrapper *) first)->node;
    }
    rtems_chain_insert (first, &chn->runp);

  }

}

/*
 *  aio_debug 
 * 
 * Prints the request list (I found it useful)
 *
 *  Input parameters: NONE
 *
 *  Output parameters: NONE
 */

void
rtems_aio_debug (void)
{

  rtems_chain_node *node;
  rtems_chain_node *aux;
  rtems_chain_control *chain;
  rtems_aio_chain *perfd;
  rtems_aio_request *perprio;

  node = aio_requestlist.first;

  while (!rtems_chain_is_tail (&aio_requestlist, node)) {
    perfd = (rtems_aio_chain *) node;
    chain = &perfd->perfd;

    printf ("FD %d -> ", perfd->fd);

    aux = chain->first;

    while (!rtems_chain_is_tail (chain, aux)) {
      perprio = (rtems_aio_request *) aux;

      printf (" { %d } ", perprio->aiocbp->aio_reqprio);

      aux = aux->next;

    }

    printf ("\n");

    node = node->next;

  }

}
